/*
 * particle_filter.h
 *
 * 2D particle filter class.
 */

#ifndef PARTICLE_FILTER_H_
#define PARTICLE_FILTER_H_
#include <vector>
#include <string>
#include <base/defines.h>

class DbgFrame;

namespace Filtering
{
    struct Particle
    {
        int id;
        double x;
        double y;
        double z;
        double theta;
        double weight;
    };

    class ParticleFilter
    {
        // Number of particles to draw
        int num_particles_;

        // Flag, if filter is initialized
        bool is_initialized;

        // Vector of weights of all particles
        std::vector<double> weights;

    public:
        // Set of current particles
        std::vector<Particle> particles;

        // Constructor
        // @param M Number of particles
        ParticleFilter() : num_particles_(0), is_initialized(false) {}

        // Destructor
        ~ParticleFilter() {}


        void Print(DbgFrame* dbg_frame);
        /**
	 * init Initializes particle filter by initializing particles to Gaussian
	 *   distribution around first position and all the weights to 1.
	 * @param x Initial x position [m] (simulated estimate from GPS)
	 * @param y Initial y position [m]
	 * @param theta Initial orientation [rad]
	 * @param std[] Array of dimension 3 [standard deviation of x [m], standard deviation of y [m]
	 *   standard deviation of yaw [rad]]
	 */
        void init(double x, double y, double z, double theta, double std[], int num_particles);

        /**
	 * prediction Predicts the state for the next time step
	 *   using the process model.
	 * @param delta_t Time between time step t and t+1 in measurements [s]
	 * @param std_pos[] Array of dimension 3 [standard deviation of x [m], standard deviation of y [m]
	 *   standard deviation of yaw [rad]]
	 * @param velocity Velocity of car from t to t+1 [m/s]
	 * @param yaw_rate Yaw rate of car from t to t+1 [rad/s]
	 */
        void prediction(double delta_t, double std_pos[], double velocity, double yaw_rate);

        /**
	 * updateWeights Updates the weights for each particle based on the likelihood of the 
	 *   observed measurements. 
	 * @param sensor_range Range [m] of sensor
	 * @param std_landmark[] Array of dimension 2 [standard deviation of range [m],
	 *   standard deviation of bearing [rad]]
	 * @param observations Vector of landmark observations
	 * @param map Map class containing map landmarks
	 */
        void updateWeights(double x, double y, double z, double bearing, double std_landmark[]);

        /**
	 * resample Resamples from the updated set of particles to form
	 *   the new set of particles.
	 */
        void resample();

        /*
	 * write Writes particle positions to a file.
	 * @param filename File to write particle positions to.
	 */
        void write(std::string filename);

        /**
	 * initialized Returns whether particle filter is initialized yet or not.
	 */
        const bool initialized() const
        {
            return is_initialized;
        }

        bool GetCurLoc(GeoPoint& gpsPoint);

        bool GetCurrentLoc(double& x, double& y, double& z, double& theta);
    };
} // namespace Filtering

#endif /* PARTICLE_FILTER_H_ */
